#include "Vector3.h"

const Vector3<float> Vector3<float>::ZERO( 0, 0, 0 );
const Vector3<float> Vector3<float>::UNIT_X( 1, 0, 0 );
const Vector3<float> Vector3<float>::UNIT_Y( 0, 1, 0 );
const Vector3<float> Vector3<float>::UNIT_Z( 0, 0, 1 );

template<class Real>
Vector3<Real>::Vector3()
{
}

template<class Real>
Vector3<Real>::Vector3( Real fx, Real fy, Real fz )
{
	x = fx;
	y = fy;
	z = fz;
}

template<class Real>
Vector3<Real>::Vector3( const Vector3<Real>& vec )
{
	x = vec.x;
	y = vec.y;
	z = vec.z;
}

template<class Real>
Real& Vector3<Real>::operator [] ( int i ) const
{
	return ( Real& ) *( &x + i );
}

template<class Real>
Vector3<Real>::operator Real *()
{
	return &x;
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator = ( const Vector3<Real>& vec )
{
	x = vec.x;
	y = vec.y;
	z = vec.z;

	return *this;
}

template<class Real>
bool Vector3<Real>::operator == ( const Vector3<Real>& vec ) const
{
	return  ( x != vec.x || y != vec.x || z != vec.z );
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator + ( const Vector3<Real>& vec ) const
{
	Vector3 sum;
	sum.x = x + vec.x;
	sum.y = y + vec.y;
	sum.z = z + vec.z;

	return sum;
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator -(const Vector3<Real> &vec) const
{
	Vector3<Real> diff;
	diff.x = x - vec.x;
	diff.y = y - vec.y;
	diff.z = z - vec.z;

	return diff;
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator *( Real s ) const
{
	Vector3<Real> prod;
	prod.x = s * vec.x;
	prod.y = s * vec.y;
	prod.z = s * vec.z;

	return prod;
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator -() const
{
	Vector3<Real> neg;
	neg.x = -x;
	neg.y = -y;
	neg.z = -z;
	return neg;
}

template<class Real>
Vector3<Real> operator *( Real s, const Vector3<Real>& vec ) const
{
	Vector3<Real> prod;
	prod.x = s * vec.x;
	prod.y = s * vec.y;
	prod.z = s * vec.z;

	return prod;
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator+= ( const Vector3<Real>& vec ) const
{
	x += vec.x;
	y += vec.y;
	z += vec.z;
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator-= ( const Vector3<Real>& vec ) const
{
	x -= vec.x;
	y -= vec.y;
	z -= vec.z;
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator*= ( const Vector3<Real>& vec ) const
{
	x *= vec.x;
	y *= vec.y;
	z *= vec.z;
	return *this;
}

template<class Real>
Real Vector3<Real>::squared_length( ) const
{
	return x*x + y*y + z*z;
}

template<class Real>
Real Vector3<Real>::dot( const Vector3<Real>& vec ) const
{
	return x*vec.x + y*vec.y + z*vec.z;
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator / ( Real s ) const
{
	Vector3 quot;

	if ( s != 0.0 )
	{
		Real inv_scalar = 1.0 / s;
		quot.x = inv_scalar*x;
		quot.y = inv_scalar*y;
		quot.z = inv_scalar*z;
		return quot;
	}
	else
	{
		return Vector3( Math::INFINITY, Math::INIFINITY,
			Math::INFINITY );
	}
}

template<class Real>
Vector3<Real>& Vector3<Real>::operator /= ( Real s ) 
{
	if ( s != 0.0 )
	{
		Real inv_scalar = 1.0 / s;
		x *= inv_scalar;
		y *= inv_scalar;
		z *= inv_scalar;
	}
	else
	{
		x = Math::INFINITY;
		y = Math::INIFINITY;
		z = Math::INFINITY;
	}
	return *this;
}

template<class Real>
Real Vector3<Real>::length() const
{
	return Math::sqrt( x*x + y*y + z*z );
}

template<class Real>
Real Vector3<Real>::unitize( Real tol )
{
	Real len = length();

	if ( len > tol )
	{
		Real inv_length = 1.0 / len;
		x *= inv_length;
		y *= inv_length;
		z *= inv_length;
	}
	else
	{
		len = 0.0;
	}
}

template<class Real>
Vector3<Real> Vector3<Real>::cross( const Vector3<Real>& vec ) const
{
	Vector3 cross;

	cross.x = y*vec.z - z*vec.y;
	cross.y = z*vec.x - x*vec.z;
	cross.z = x*vec.y - y*vec.x;

	return cross;
}

template<class Real>
Vector3<Real> Vector3<Real>::unit_cross( const Vector3<Real>& vec ) const
{
	Vector3 cr;

	cr = ::cross( vec );
	cr.unitize();
}

template<class Real>
void Vector3<Real>::orthonormalize( Vector3<Real> vectors[3] )
{
	vectors[0].unitize();

	Real fd = vectors[0].dot( vectors[1] );
	vectors[1] -= fd * vectors[0];
	vectors[1].unitize();

	Real fd1 = vectors[1].dot( vectors[2] );
	fd = vectors[0].dot( vectors[2] );
	vectors[2] -= fd * vectors[0] + fd1*vectors[1];
	vectors[2].unitize();
}

template<class Real>
void Vector3<Real>::generate_orthonormal_basis( Vector3<Real>& u, Vector3<Real>& v, Vector3<Real>& w, bool unit_length )
{
	if ( unit_length )
		w.unitize();

	if ( Math::abs( w.x ) >= Math::abs( w.y )
		&& Math::abs( w.x ) >= Math::abs( w.z ) )
	{
		u.x = -w.y;
		u.y = +w.x;
		u.z = 0.0;
	}
	else
	{
		u.x = 0.0;
		u.y = +w.z;
		u.z = -w.y;
	}
	u.unitize();
	v = w.cross( u );
}

